#ifndef _SERVO_CONTROL_H
#define _SERVO_CONTROL_H
#include "stm32f4xx_hal.h"

#define MAX_ROT_ANGLE_PER_MS 0.188f
#define SV_QUEUE_LENGTH 10
typedef struct servo_task{
	float s_angle[4];
	float s_vel[4];
	uint8_t keep_prev[4];
} SVT;

void Get_Servo_Control_Msg(void);

uint32_t angle_conv(uint8_t servo_num,float angle);
void servo_init(float s0_angle,float s1_angle,float s2_angle,float s3_angle);
void servo_edit_next_task(uint8_t servo_num,float angle,float velocity);
void servo_confirm_next_task(void);

uint8_t arm_motion(uint8_t type);
void servo_set(uint8_t servo_num,float angle,float velocity);

extern float servo_current_angle[4];
extern float servo_expected_angle[4];
extern float servo_velocity[4];
extern uint8_t arm_is_moving;
extern SVT svqueue[SV_QUEUE_LENGTH];
extern uint8_t svthead;
extern uint8_t svtrear;
#endif
